# -*- coding: UTF-8 -*-
import almath
from naoqi import *
import time
import  math

Config = [["MaxStepX", 0.06],
           ["MaxStepY", 0.13],
           ["MaxStepTheta", 0.4],
           ["MaxStepFrequency", 1],
           ["StepHeight", 0.03],
           ["TorsoWx", 0.0],
           ["TorsoWy", 0.0]]
ip = "192.168.1.9"
move = ALProxy("ALMotion",ip,9559)
posture = ALProxy("ALRobotPosture",ip,9559)
move.wakeUp()
posture.goToPosture("StandInit",0.3)
time.sleep(3)

try:
    memoryProxy = ALProxy("ALMemory", ip, 9559)
except Exception, e:
    print "Could not create proxy to ALMemory"

    print "Error was: ",


x = 0.2
y = 0.00
theta = 0.0
try:
    move.angleInterpolation("HeadPitch", 0.01, 1, True)
    move.angleInterpolation("HeadYaw", -0.00, 1, False)


    while 1:
        a = input()
        if a == 9:
            move.move(0, 0, -math.pi/16)

        elif a == 7:
            move.move(0, 0, math.pi/16)
        elif a == 8:
            #move.move(0.07, 0, 0)
            move.moveTo(0.07, 0.0, 0.0, Config)
        elif a == 88:
            #move.move(0.06, 0, 0)
            move.moveTo(0.05, 0.0, 0.0, Config)

        elif a == 0:
            #move.move(0.1, 0, 0)
            move.moveTo(0.1, 0.0, 0.0, Config)
        elif a == 99:#zuoxie
            move.moveTo(1, -0.7, 0, Config)
        elif a == 77:
            move.moveTo(0.76, 0.8, 0, Config)
        elif a == 6:
            move.move(0, -0.24, 0, Config)
        elif a == 66:
            move.move(0.3, -0.7, 0, Config)
        elif a == 4:
            move.move(0, 0.2, 0, Config)
        elif a == 1:
            #move.angleInterpolation("HeadPitch", 0.5, 1, True)
            move.angleInterpolation("HeadYaw", 30*almath.TO_RAD, 1, False)
            move.angleInterpolation("HeadYaw", -30 * almath.TO_RAD, 1, False)
        elif a == 3:
            move.move(0.2, -0.2, 0, Config)
        elif a == 5:
            move.stopMove()
        else:
            continue
            # move.rest()
            # break45

except Exception, errorMsg:
    print str(errorMsg)
    print "This example is not allowed on this robot."
    exit()
